# 附有条件的间接平差 Adjustment of observations with condition equations
from numpy import mat, array, deg2rad, sin, cos
from MathUtils import *

# 点名
A = 'A'
B = 'B'
C = 'C'
D = 'D'
E = 'E'
F = 'F'
G = 'G'

# 边名
AB = 'AB'
BC = 'BC'
CD = 'CD'
DF = 'DF'
FG = 'FG'
AG = 'AG'
AE = 'AE'
BE = 'BE'
CE = 'CE'
DE = 'DE'
EF = 'EF'
EG = 'EG'


# 前方交会
def forwardIntersection(A, B, alpha, beta):
	xa, ya = A
	xb, yb = B
	alpha = deg2rad(alpha)
	beta = deg2rad(beta)
	xp = (xa*cot(beta) + xb*cot(alpha) - ya + yb) / (cot(alpha) + cot(beta))
	yp = (ya*cot(beta) + yb*cot(alpha) + xa - xb) / (cot(alpha) + cot(beta))

	return array([xp, yp])


# 求边长
def calcDistance(P1, P2):
	return np.sqrt((P2 - P1).dot(P2 - P1))


# 误差方程系数
def coeA(s):
	rho = 206265
	return rho*sin(deg2rad(s[1]))/s[0]


def coeB(s):
	rho = 206265
	return -rho*cos(deg2rad(s[1]))/s[0]


np.set_printoptions(linewidth=400)
if __name__ == '__main__':
	# 点集合
	P = {}
	# 边集合
	S = {}
	# 角度观测值
	L = []

	# 读取角度观测值
	with open("OecAdj_AngleObservations.txt", "r") as f:
		while True:
			line = f.readline()
			if line == "":
				break
			d, m, s = line.split(",")
			d = float(d)
			m = float(m)
			s = float(s)
			L.append(dms2dec((d, m, s)))
		f.close()
	L = np.array(L)

	# 已知坐标
	P[A] = array([2794005.704, 19433831.155])
	P[B] = array([2802234.190, 19437826.220])

	# 计算近似坐标
	P[E] = forwardIntersection(P[A], P[B], L[1-1], L[2-1])
	P[C] = forwardIntersection(P[E], P[B], L[6-1], L[4-1])
	P[D] = forwardIntersection(P[E], P[C], L[9-1], L[7-1])
	P[F] = forwardIntersection(P[E], P[D], L[12-1], L[10-1])
	P[G] = forwardIntersection(P[E], P[F], L[15-1], L[13-1])

	print("近似坐标：")
	for p in P:
		print(p, P[p])

	# 计算近似边长和近似方位角
	S[AB] = [calcDistance(P[A], P[B]), azimuth(P[A], P[B])]
	S[BC] = [calcDistance(P[B], P[C]), azimuth(P[B], P[C])]
	S[CD] = [calcDistance(P[C], P[D]), azimuth(P[C], P[D])]
	S[DF] = [calcDistance(P[D], P[F]), azimuth(P[D], P[F])]
	S[FG] = [calcDistance(P[F], P[G]), azimuth(P[F], P[G])]
	S[AG] = [calcDistance(P[A], P[G]), azimuth(P[A], P[G])]
	S[AE] = [calcDistance(P[A], P[E]), azimuth(P[A], P[E])]
	S[BE] = [calcDistance(P[B], P[E]), azimuth(P[B], P[E])]
	S[CE] = [calcDistance(P[C], P[E]), azimuth(P[C], P[E])]
	S[DE] = [calcDistance(P[D], P[E]), azimuth(P[D], P[E])]
	S[EF] = [calcDistance(P[E], P[F]), azimuth(P[E], P[F])]
	S[EG] = [calcDistance(P[E], P[G]), azimuth(P[E], P[G])]

	print("近似边长和方位角：")
	for s in S:
		print(s, S[s])

	Bmat = np.mat([
		[0, 0, 0, 0, -0.1569, -0.2839, 0, 0, 0, 0],
		[0, 0, 0, 0, 0.2555, -0.0962, 0, 0, 0, 0],
		[0, 0, 0, 0, -0.0986, 0.3801, 0, 0, 0, 0],
		[0.3341, 0.1753, 0, 0, -0.2555, -0.0962, 0, 0, 0, 0],
		[-0.4777, 0.1596, 0, 0, 0.1436, -0.3349, 0, 0, 0, 0],
		[0.1436, -0.3349, 0, 0, 0.1119, 0.2387, 0, 0, 0, 0],
		[-0.1673, -0.3923, 0.3109, 0.0547, -0.1436, 0.3349, 0, 0, 0, 0],
		[0.3109, 0.0547, -0.1629, 0.1686, -0.1480, -0.2260, 0, 0, 0, 0],
		[-0.1436, 0.3349, -0.1480, -0.226, 0.2916, -0.1089, 0, 0, 0, 0],
		[0, 0, -0.2325, 0.0164, 0.1480, -0.2260, 0, 0, 0.0845, -0.2424],
		[0, 0, 0.0845, -0.2424, -0.2930, -0.0515, 0, 0, 0.2085, 0.1909],
		[0, 0, 0.1480, 0.2260, 0.1405, -0.2775, 0, 0, -0.2930, -0.0515],
		[0, 0, 0, 0, 0.2930, -0.0151, -0.2296, -0.2425, -0.0634, 0.2940],
		[0, 0, 0, 0, -0.1366, 0.3088, 0.3662, -0.0583, -0.2296, -0.2425],
		[0, 0, 0, 0, -0.1564, -0.2493, -0.1366, 0.3008, 0.2930, -0.0515],
		[0, 0, 0, 0, 0.1366, -0.3008, 0.2278, 0.2931, 0, 0],
		[0, 0, 0, 0, 0.1569, 0.2839, -0.3644, 0.0077, 0, 0],
		[0, 0, 0, 0, -0.2935, 0.0169, 0.1366, -0.3008, 0, 0]
	])

	l = mat([0.54, 1.55, 0.41, 0.04, 1.26, -0.62, -0.36, -1.19, 0.63, 0.22, 1.53, -0.29, -0.93, -0.47, -0.56, -0.56, -1.77, -0.69]).transpose()

	Power = np.eye(18)

	Nbb = Bmat.transpose()*Power*Bmat
	W = Bmat.transpose()*Power*l

	xh = np.linalg.inv(Nbb)*W

	print(xh)

	K = np.linalg.inv(Nbb)*(W-Bmat*xh)

	Ks = mat([-0.01, 0.04]).transpose()

	X0 = mat([P[C][0], P[C][1], P[D][0], P[D][1], P[E][0], P[E][1], P[F][0], P[F][1], P[G][0], P[G][1]]).transpose()

	V = Bmat*xh - l

	Lh = L + V

	sigma0 = np.sqrt(V.transpose()*Power*V / 10)
	Qff = K * P ^ (-1) * np.linalg.inv(K)
	Qfe = K * P ^ (-1) * np.linalg.inv(Ks)
	QFF = 10.8513
	SigmaXf = 1.2 * np.sqrt(QFF)
	SigmaYf = 1.2 * np.sqrt(Qfe)

	print()
